#ifndef INNFOS_KINEMATICS_H
#define INNFOS_KINEMATICS_H

#include <Eigen/Eigen>
#include <vector>

using namespace Eigen;
using Vector6 = Matrix<double, 1, 6>;

class innfos_kinematics {

public:
    innfos_kinematics();
	~innfos_kinematics();

	double joint_maximum[6] = {150*M_PI/180, 95*M_PI/180, 150*M_PI/180, 150*M_PI/180, 150*M_PI/180, M_PI};
	double joint_minimum[6] = {-150*M_PI/180, -95*M_PI/180, -150*M_PI/180, -150*M_PI/180, -150*M_PI/180, -M_PI};
	double ratio_for_joints_in_ik[6] = {10, 10, 5, 1, 1, 1};

    Isometry3d baseTend(Vector6 & q);
    int solve(const Isometry3d& transform3D, std::vector<Vector6>& joints, double eps=1e-10);
    int getQ(const Isometry3d& transform3D, Vector6 &cur_Q, Vector6 &targetQ);
    double norm2(Vector6 q1, Vector6 q2);

private:
    Matrix<double,7,1> _a;
    Matrix<double,7,1> _alpha;
    Matrix<double,7,1> _d;
    Matrix<double,7,1> _theta;

    Matrix<double, 3, 3> r;
    Matrix<double, 3, 1> p;

    double d2;
    double a3;
    double a4;
    double d5;
    double d6;
    double d7;

    double _eps;

    Isometry3d Transform3D_DH(double a,double alpha,double d, double theta);

    double solveQ0(bool positive);
    double solveQ4(double q0,bool positive);
    double solveQ5(double q0,double q4);
    double solveQ123(double q0,double q4);
    double solveQ2(double q0,double q4,double q123,bool positive);
    double solveQ1(double q0,double q4,double q123,double q2);
    double solveQ3(double q123,double q2,double q1);
    double M(double q0, double q4,double q123);
    double N(double q0, double q4,double q123);

	int filter_joints(const Vector6 &cQ, const Vector6 &tQ, double threahold = 6.283);
};

inline double innfos_kinematics::norm2(Vector6 q1, Vector6 q2)
{
	Vector6 q = q2 - q1;
    double res = 0;
    for (int i=0; i < 5; i++)
    {
    	res = pow(q[i],2) * ratio_for_joints_in_ik[i] + res;
    }
    return sqrt(res);
}



#endif // INNFOS_KINEMATICS_H
